#include "CRRTTree.h"

#include <iostream>
#include <random>
#include <algorithm>
#include <string.h>
#include <QString>

CRRTTree::CRRTTree(QObject *parent) : QObject(parent)
{

}

/**
 * @brief  Construct a new CRRTTree::CRRTTree object
 * @param  goal_bias        
 * @param  step_size        
 * @param  max_step_size    
 * @author Jimmy 
 * @date   2023.03.04
 */
CRRTTree::CRRTTree(double goal_bias, double step_size, double max_step_size) :
                                        m_goal_bias(goal_bias),
                                        m_step_size(step_size),
                                        m_max_step_size(max_step_size)
{
    
}

/**
 * @brief  树节点生长
 * @param  node             
 * @author Jimmy 
 * @date   2023.03.05
 */
void CRRTTree::grow(const CRRTNode &node)
{
    m_nodes.emplace_back(node);
}

/**
 * @brief  获取最终路径
 * @return std::vector<CPoint2D> 
 * @author Jimmy 
 * @date   2023.03.05
 */
std::vector<CPoint2D> CRRTTree::getPath()
{
    qDebug("[CRRTTree] getPath\n");

    std::vector<CPoint2D> path;
    CRRTNode * node = &m_nodes.back();

    CPoint2D start_point = m_nodes.front().getPoint();
    
    while (node != nullptr)
    {
        CPoint2D point = node->getPoint();
        path.push_back(point);
        node = node->getParent();

        if (node == nullptr || point == start_point)
        {
            qDebug("[CRRTTree] getPath error\n");
            break;
        }
    }

    qDebug("[CRRTTree] getPath success\n");
    return path;
}


/**
 * @brief  产生新的随机点
 * @return CPoint2D 
 * @author Jimmy 
 * @date   2023.03.04
 */
CPoint2D CRRTTree::generateRandomPoint()
{
    unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();;
    std::mt19937 eng(seed);
    std::uniform_int_distribution<int> x_range(0, m_size_x);
    std::uniform_int_distribution<int> y_range(0, m_size_y);

    int x_value = x_range(eng);
    int y_value = y_range(eng);

    return CPoint2D(x_value, y_value);
}

/**
 * @brief  生成偏移的点
 * @param  start_point      
 * @param  end_point        
 * @return CPoint2D 
 * @author Jimmy 
 * @date   2023.03.05
 */
CPoint2D CRRTTree::generateOffsetPoint(const CPoint2D &start_point, const CPoint2D &end_point)
{
    double theta = std::atan2(end_point.getY() - start_point.getY(), end_point.getX() - start_point.getX());

    double distance = start_point.getEuclideanDistance(end_point);

    double delta_x = 0.0;
    double delta_y = 0.0;
    if (distance > m_max_step_size)
    {
        delta_x = m_max_step_size * std::cos(theta);
        delta_y = m_max_step_size * std::sin(theta);
    }
    else if (distance <= m_max_step_size && distance > m_step_size)
    {
        delta_x = distance * std::cos(theta);
        delta_y = distance * std::sin(theta);
    }
    else
    {
        delta_x = m_step_size * std::cos(theta);
        delta_y = m_step_size * std::sin(theta);
    }

    CPoint2D offset_new_pt(start_point.getX() + (int)delta_x, start_point.getY() + (int)delta_y);

    return offset_new_pt;
}

/**
 * @brief  生成树节点
 * @return CRRTNode 
 * @author Jimmy 
 * @date   2023.03.04
 */
CRRTNode CRRTTree::generateTreeNode(bool &state)
{
    auto nodes_copy = m_nodes;
    auto &nodes = nodes_copy;
    auto f = [nodes](int i) -> CPoint2D {return nodes.at(i).getPoint();};
    CKDTree kd_tree(f, nodes.size());

    CPoint2D new_random_point = generateRandomPoint();
    CPoint2D nearest_point = kd_tree.nearest(new_random_point);

    CPoint2D offset_point = generateOffsetPoint(nearest_point, new_random_point);

    auto nearest_itr = std::find_if(m_nodes.begin(), m_nodes.end(), \
                [nearest_point] (const CRRTNode &node)->bool {return node.getPoint() == nearest_point;});

    if (nearest_itr != m_nodes.end())
    {
        int index = nearest_itr - m_nodes.begin();
        CRRTNode new_random_node(offset_point, &m_nodes.at(index));
        state = true;
        return new_random_node;
    }

    state = false;
    return CRRTNode();
}
